#include "AddAttachedSensorDialog.h"
#include "ui_AddAttachedSensorDialog.h"

#include <MMM/RapidXML/RapidXMLReader.h>
#include <MMM/RapidXML/RapidXMLWriter.h>

#include <VirtualRobot/RobotConfig.h>
#include <MMMSimoxTools/MMMSimoxTools.h>

#include <QCheckBox>
#include <QLineEdit>
#include <QMessageBox>
#include <QFileDialog>
#include <QSpinBox>
#include <set>
#include <regex>

AddAttachedSensorDialog::AddAttachedSensorDialog(MMM::BaseAddAttachedSensorConfigurationPtr configuration, QWidget* parent) :
    QDialog(parent),
    configuration(configuration),
    added(false),
    ui(new Ui::AddAttachedSensorDialog)
{
    ui->setupUi(this);

    connect(ui->LoadFileButton, SIGNAL(clicked()), this, SLOT(loadFile()));
    connect(ui->ChooseMotionBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setCurrentMotion(int)));
    connect(ui->LoadSensorConfigButton, SIGNAL(clicked()), this, SLOT(loadConfigurationFile()));
    connect(ui->SaveSensorConfigButton, SIGNAL(clicked()), this, SLOT(saveConfigurationFile()));
    connect(ui->PreviewButton, SIGNAL(clicked()), this, SLOT(preview()));
    connect(ui->ConvertButton, SIGNAL(clicked()), this, SLOT(convert()));
    connect(ui->AddButton, SIGNAL(clicked()), this, SLOT(add()));
    connect(ui->CancelButton, SIGNAL(clicked()), this, SLOT(hide()));

    bool ignoreFirstLine;
    std::string firstSplitChar;
    bool byRegex;
    std::string secondSplitChar;
    std::string regex;
    std::vector<int> indexes;
    configuration->loadConfiguration(ignoreFirstLine, firstSplitChar, byRegex, secondSplitChar, regex, indexes);

    if (ignoreFirstLine) ui->IgnoreFirstLine->setChecked(1);
    ui->FirstDelimiter->setText(QString::fromStdString(firstSplitChar));
    if (byRegex) ui->ByRegex->setChecked(1);
    else ui->ByDelimiter->setChecked(1);
    ui->SecondDelimiter->setText(QString::fromStdString(secondSplitChar));
    ui->Regex->setText(QString::fromStdString(regex));

    QGridLayout* indexGrid = new QGridLayout();
    ui->IndexGroupBox->setLayout(indexGrid);
    int index = 0;
    for(const std::string &name : configuration->getIndexNames()) {
        indexGrid->addWidget(new QLabel(QString::fromStdString(name)), index, 0);
        QSpinBox* spin = new QSpinBox();
        spin->setValue(indexes[index]);
        indexGrid->addWidget(spin, index, 1);
        indexSpinner.push_back(spin);
        index++;
    }

    this->setWindowTitle(QString::fromStdString("Add " + configuration->getSensorName() + " sensor"));

    loadFile(settings.value(QString::fromStdString(configuration->getName() + "/sensorDataFilePath"), QString::fromStdString("")).toString().toStdString());
}

AddAttachedSensorDialog::~AddAttachedSensorDialog() {
    delete ui;
}

bool AddAttachedSensorDialog::addSensor(MMM::MotionList motions) {
    loadMotions(motions);
    std::string configurationFilePath = settings.value(QString::fromStdString(configuration->getName() + "/sensorConfigurationFilePath"), QString::fromStdString("")).toString().toStdString();
    MMM_INFO << "Trying to load sensor configuration from " << configurationFilePath << std::endl;
    loadConfigurationFile(configurationFilePath);
    added = false;
    exec();
    return added;
}

void AddAttachedSensorDialog::loadFile() {
    std::string sensorFilePath = QFileDialog::getOpenFileName(this, tr("Load sensor file"), QString::fromStdString(this->dataFilePath), tr("Sensor file (*.txt *.xml *.csv)")).toStdString();
    loadFile(sensorFilePath);
}

void AddAttachedSensorDialog::loadFile(const std::string &dataFilePath) {
    if (dataFilePath.empty()) return;

    ui->PlainSensorFilePreview->clear();
    sensorFile.clear();
    std::ifstream ifs(dataFilePath);
    sensorFile.assign(std::istreambuf_iterator<char>(ifs), std::istreambuf_iterator<char>());
    if (sensorFile.empty()) return;
    ui->ConvertButton->setEnabled(true);
    ui->PreviewButton->setEnabled(true);
    ui->PlainSensorFilePreview->appendPlainText(sensorFile.c_str());
    ui->PlainSensorFilePreview->setLineWrapMode(QPlainTextEdit::NoWrap);
    ui->PlainSensorFilePreview->moveCursor(QTextCursor::Start);
    ui->SensorFilePath->setText(QString::fromStdString(dataFilePath));
    ui->SensorFilePath->setToolTip(QString::fromStdString(dataFilePath));
    settings.setValue(QString::fromStdString(configuration->getName() + "/sensorDataFilePath"), QString::fromStdString(dataFilePath));
    this->dataFilePath = dataFilePath;
}

void AddAttachedSensorDialog::loadConfigurationFile() {
    std::string sensorConfigurationFilePath = QFileDialog::getOpenFileName(this, tr("Load sensor configuration file"), QString::fromStdString(this->configurationFilePath), tr("IMU configuration file (*.xml)")).toStdString();
    loadConfigurationFile(sensorConfigurationFilePath);
}

void AddAttachedSensorDialog::loadConfigurationFile(const std::string &configurationFilePath) {
    if (configurationFilePath.empty()) return;

    try {
        MMM::RapidXMLReaderNodePtr configuration = MMM::RapidXMLReader::FromFile(configurationFilePath)->getRoot();
        MMM::IAttachedSensorPtr attachedSensor = this->configuration->loadSensorConfiguration(configuration);
        bool segmentNameFound = false;
        for (int i = 0; i < ui->SegmentName->count(); i++) {
            if (ui->SegmentName->itemText(i).toStdString() == attachedSensor->getSegment()) {
                ui->SegmentName->setCurrentIndex(i);
                segmentNameFound = true;
                break;
            }
        }
        if (!segmentNameFound) {
            error("No segment with name " + attachedSensor->getSegment() + " found in model of motion " + currentMotion->getName());
            return;
        }
        for (int i = 0; i < ui->OffsetUnit->count(); i++) {
            if (ui->OffsetUnit->itemText(i).toStdString() == attachedSensor->getOffsetUnit()) {
                ui->OffsetUnit->setCurrentIndex(i);
                break;
            }
        }
        Eigen::Matrix4f transformationMatrix = attachedSensor->getOffset();
        ui->OffsetMatrix_A11->setText(QString::number(transformationMatrix(0,0)));
        ui->OffsetMatrix_A12->setText(QString::number(transformationMatrix(0,1)));
        ui->OffsetMatrix_A13->setText(QString::number(transformationMatrix(0,2)));
        ui->OffsetMatrix_A14->setText(QString::number(transformationMatrix(0,3)));
        ui->OffsetMatrix_A21->setText(QString::number(transformationMatrix(1,0)));
        ui->OffsetMatrix_A22->setText(QString::number(transformationMatrix(1,1)));
        ui->OffsetMatrix_A23->setText(QString::number(transformationMatrix(1,2)));
        ui->OffsetMatrix_A24->setText(QString::number(transformationMatrix(1,3)));
        ui->OffsetMatrix_A31->setText(QString::number(transformationMatrix(2,0)));
        ui->OffsetMatrix_A32->setText(QString::number(transformationMatrix(2,1)));
        ui->OffsetMatrix_A33->setText(QString::number(transformationMatrix(2,2)));
        ui->OffsetMatrix_A34->setText(QString::number(transformationMatrix(2,3)));
        ui->OffsetMatrix_A41->setText(QString::number(transformationMatrix(3,0)));
        ui->OffsetMatrix_A42->setText(QString::number(transformationMatrix(3,1)));
        ui->OffsetMatrix_A43->setText(QString::number(transformationMatrix(3,2)));
        ui->OffsetMatrix_A44->setText(QString::number(transformationMatrix(3,3)));

        settings.setValue(QString::fromStdString(this->configuration->getName() + "/sensorConfigurationFilePath"), QString::fromStdString(configurationFilePath));
        this->configurationFilePath = configurationFilePath;
    } catch (MMM::Exception::XMLFormatException &e) {
        error(e.what());
    }
}

void AddAttachedSensorDialog::saveConfigurationFile() {
    std::string sensorConfigurationFilePath = QFileDialog::getSaveFileName(this, tr("Save sensor configuration file"), QString::fromStdString("imuConfiguration.xml"), tr("IMU configuration file (*.xml)")).toStdString();
    if (!sensorConfigurationFilePath.empty()) {
        try {
            Eigen::Matrix4f offset = getTransformationMatrix();
            std::string segment = ui->SegmentName->currentText().toStdString();
            std::string offsetUnit = ui->OffsetUnit->currentText().toStdString();
            configuration->setConfiguration(segment, offset, offsetUnit);
            configuration->getAttachedSensor()->safeSensorConfiguration(sensorConfigurationFilePath);
        } catch(MMM::Exception::MMMException &e) {
            error(e.what());
            return;
        }
    }
}

Eigen::Matrix4f AddAttachedSensorDialog::getTransformationMatrix() {
    Eigen::Matrix4f transformationMatrix;
    bool ok;
    transformationMatrix(0,0) = ui->OffsetMatrix_A11->text().toFloat(&ok);
    transformationMatrix(0,1) = ui->OffsetMatrix_A12->text().toFloat(&ok);
    transformationMatrix(0,2) = ui->OffsetMatrix_A13->text().toFloat(&ok);
    transformationMatrix(0,3) = ui->OffsetMatrix_A14->text().toFloat(&ok);
    transformationMatrix(1,0) = ui->OffsetMatrix_A21->text().toFloat(&ok);
    transformationMatrix(1,1) = ui->OffsetMatrix_A22->text().toFloat(&ok);
    transformationMatrix(1,2) = ui->OffsetMatrix_A23->text().toFloat(&ok);
    transformationMatrix(1,3) = ui->OffsetMatrix_A24->text().toFloat(&ok);
    transformationMatrix(2,0) = ui->OffsetMatrix_A31->text().toFloat(&ok);
    transformationMatrix(2,1) = ui->OffsetMatrix_A32->text().toFloat(&ok);
    transformationMatrix(2,2) = ui->OffsetMatrix_A33->text().toFloat(&ok);
    transformationMatrix(2,3) = ui->OffsetMatrix_A34->text().toFloat(&ok);
    transformationMatrix(3,0) = ui->OffsetMatrix_A41->text().toFloat(&ok);
    transformationMatrix(3,1) = ui->OffsetMatrix_A42->text().toFloat(&ok);
    transformationMatrix(3,2) = ui->OffsetMatrix_A43->text().toFloat(&ok);
    transformationMatrix(3,3) = ui->OffsetMatrix_A44->text().toFloat(&ok);
    if (ok) return transformationMatrix;
    else throw MMM::Exception::MMMException("Could not convert transformation matrix!");
}

void AddAttachedSensorDialog::setSensorConfiguration() {
    try {
        Eigen::Matrix4f offset = getTransformationMatrix();
        std::string segment = ui->SegmentName->currentText().toStdString();
        std::string offsetUnit = ui->OffsetUnit->currentText().toStdString();
        std::string uniqueName = ui->SensorName->text().toStdString();
        std::string description = ui->SensorDescription->text().toStdString();
        configuration->setConfiguration(segment, offset, offsetUnit, description, uniqueName);
    } catch(MMM::Exception::MMMException &e) {
        MMM_ERROR << e.what() << std::endl;
        error(e.what());
    }
}

void AddAttachedSensorDialog::preview() {
    ui->ConvertedFilePreview->clear();
    std::vector<int> indexes;
    for (QSpinBox* spin : indexSpinner) {
        indexes.push_back(spin->value());
    }
    std::string previewText;
    try {
        previewText = configuration->getPreview(sensorFile, ui->IgnoreFirstLine->isChecked(), ui->FirstDelimiter->text().toStdString(), ui->ByRegex->isChecked(), ui->SecondDelimiter->text().toStdString(), ui->Regex->text().toStdString(), indexes, ui->Timestamp->isChecked() ? ui->TimestampSpecifier->text().toStdString() : std::string());
    } catch(MMM::Exception::MMMException &e) {
        error(e.what());
        return;
    }
    ui->AddButton->setEnabled(false);
    ui->ConvertedFilePreview->setLineWrapMode(QPlainTextEdit::NoWrap);
    ui->ConvertedFilePreview->appendPlainText(previewText.c_str());
    ui->ConvertedFilePreview->moveCursor(QTextCursor::Start);
}

void AddAttachedSensorDialog::convert() {
    ui->ConvertedFilePreview->clear();
    std::vector<int> indexes;
    for (QSpinBox* spin : indexSpinner) {
        indexes.push_back(spin->value());
    }
    setSensorConfiguration();
    std::string previewText;
    try {
        previewText = configuration->addSensorMeasurements(sensorFile, ui->IgnoreFirstLine->isChecked(), ui->FirstDelimiter->text().toStdString(), ui->ByRegex->isChecked(), ui->SecondDelimiter->text().toStdString(), ui->Regex->text().toStdString(), indexes, ui->Timestamp->isChecked() ? ui->TimestampSpecifier->text().toStdString() : std::string());
    } catch(MMM::Exception::MMMException &e) {
        error(e.what());
        return;
    }

    ui->AddButton->setEnabled(true);
    ui->ConvertedFilePreview->setLineWrapMode(QPlainTextEdit::NoWrap);
    ui->ConvertedFilePreview->appendPlainText(QString::fromStdString(previewText));
}

void AddAttachedSensorDialog::add() {
    try {
        currentMotion->addSensor(configuration->getSensor(), ui->TimestepDelta->value());
        QMessageBox* msgBox = new QMessageBox(this);
        msgBox->setText(QString::fromStdString(configuration->getSensorName() + " sensor added to motion " + currentMotion->getName()));
        msgBox->exec();
        added = true;
    } catch(MMM::Exception::MMMException &e) {
        error(e.what());
    }
}

void AddAttachedSensorDialog::loadMotions(MMM::MotionList motions) {
    ui->ChooseMotionBox->clear();
    currentMotion = nullptr;
    this->motions = motions;

    int i = 0;
    for (MMM::MotionPtr motion : motions) {
        ui->ChooseMotionBox->addItem(QString::fromStdString(motion->getName()));
        if (motion->getName() == motionName) ui->ChooseMotionBox->setCurrentIndex(i);
        i++;
    }
    if (!currentMotion) setCurrentMotion(0);
}

void AddAttachedSensorDialog::setCurrentMotion(int index) {
    if (index >= 0 && (int) motions.size() > index) {
        currentMotion = motions[index];
        motionName = currentMotion->getName();

        VirtualRobot::RobotPtr robot = MMM::SimoxTools::buildModel(currentMotion->getModel());

        ui->SegmentName->clear();

        for (std::string robotNodeNames : robot->getRobotNodeNames()) {
            ui->SegmentName->addItem(QString::fromStdString(robotNodeNames));
        }
    }
}

void AddAttachedSensorDialog::error(const std::string &errorMsg) {
    MMM_ERROR << errorMsg << std::endl;
    QMessageBox* msgBox = new QMessageBox(this);
    msgBox->setText(QString::fromStdString(errorMsg));
    msgBox->exec();
}
